clear all;
close all;
xk1=[0.1;0.2];
xk2=0;
nu_1=0;nu_2=0;ut_1=0;ut_2=0;ut_3=0;
T=0.01;
for k=1:1:5000
time(k)=k*T;
yd(k)=2*sin(0.2*k*T);
dyd(k)=0.4*cos(0.2*k*T);
ddyd(k)=-0.08*sin(0.2*k*T);
d(k)=2*sin(0.1*pi*k*T)+3*sin(0.2*sqrt(k*T+1));

tSpan=[0 T];

para1=[nu_1,nu_2];      % D/A
[tt,xx]=ode45('plant',tSpan,xk1,[],para1);
xk1=xx(length(xx),:);    % A/D
x1(k)=xk1(1);
x2(k)=xk1(2);

para2=[ut_1,ut_2,ut_3];      % D/A
[tt,xx]=ode45('obv',tSpan,xk2,[],para2);
xk2=xx(length(xx),:);    % A/D
s(k)=xk2;

s1=x1(k)-yd(k);
ds1=x2(k)-dyd(k);
s2=ds1+50*s1+0.5*s1^(5/7)+s(k);
dd=-2500*s(k)-4*sign(s(k))-0.5*s(k)^(5/9)-abs(-2*x1(k)+3*(1-x1(k)^2)*x2(k))*sign(s(k))+2*x1(k)-3*(1-x1(k)^2)*x2(k);
ut(k)=2*x1(k)-3*(1-x1(k)^2)*x2(k)+ddyd(k)-50*ds1-0.5*ds1^(5/7)-dd-60*s2-0.8*s2^(5/7);
nu_1=ut(k);
nu_2=d(k);
ut_1=d(k);
ut_2=x1(k);
ut_3=x2(k);
end

plot(time,x1,'k',time,yd,'r','linewidth',2);
xlabel('Time/s');ylabel('x1');
legend('x1','x1 estimation','measured x1');

figure(2);
subplot(211);
plot(time,x1-yd,'r','linewidth',2);
xlabel('Time/s');ylabel('Estimation error of x1');
subplot(212);
plot(time,x2-dyd,'r','linewidth',2);
xlabel('Time/s');ylabel('Estimation error of x2');

figure(3);
plot(time,ut,'k','linewidth',2);
xlabel('Time/s');ylabel('Varying measurement delay time');